use super::time::{SysTime,Clocker,UClock,UClockData,UTime,SysConvertor};
use super::ls::{wleast_square,error};
use super::pair::pair;

use std::thread;
use std::time::Duration;
use std::vec;
const TRACE_SIZE: usize = 128;

pub trait ClockTracer {
    fn update(&mut self,p:pair<UTime,SysTime>) -> bool;
    fn getdata(&self) -> UClockData;
    fn error(&self) -> Option<f64>;
}

pub struct ClockTrace<T:Clocker+Default> {
    v : [pair::<UTime,SysTime>;TRACE_SIZE],
    p : usize,
    f : bool,
    e : (f64,f64),
    r : UClockData,
    c : T
}

impl<T:Clocker+Default> ClockTracer for ClockTrace<T> {
    fn update(&mut self,p:pair<UTime,SysTime>) -> bool {
        self.v[self.p] = p;
        self.p += 1;
        if !self.f && self.p==TRACE_SIZE {
            self.f = true;
        }
        self.p %= TRACE_SIZE;
        if self.f || self.p>3 {
            let o = pair(self.r.self_start,self.r.cpu_start);
            if !self.f {
                self.r = wleast_square::<UTime,SysTime>(&self.v[..self.p],self.p,o);
                self.e = error(&self.v[..self.p],&self.r);
            } else {
                self.r = wleast_square::<UTime,SysTime>(&self.v,self.p,o);
                self.e = error(&self.v,&self.r);
            }
            println!("pos {} -> error {:?}",self.p,self.e);
            self.r.quality = self.e.1;
        }
        self.f || (self.p>3)
    }

    // fn update(&mut self,p:pair<UTime,SysTime>) -> bool {
    //     self.v[self.p] = p;
    //     self.p += 1;
    //     if !self.f && self.p==TRACE_SIZE {
    //         self.f = true;
    //     }
    //     self.p %= TRACE_SIZE;
    //     if self.f || self.p>3 {
    //         if !self.f {
    //             self.r = least_square::<UTime,SysTime>(&self.v[..self.p]);
    //             self.e = error(&self.v[..self.p],&self.r);
    //         } else {
    //             self.r = least_square::<UTime,SysTime>(&self.v);
    //             self.e = error(&self.v,&self.r);
    //         }
    //         println!("pos {} -> error {:?}",self.p,self.e);
    //         self.r.quality = self.e.1;
    //     }
    //     self.f || (self.p>3)
    // }
    fn error(&self) -> Option<f64> {
        if !self.f {
            None
        } else {
            Some(error(&self.v,&self.r).1)
        }
    }
    fn getdata(&self) -> UClockData {
        self.r.clone()
    }
}

impl<T:Clocker+Default> Default for ClockTrace<T> {
    fn default() -> Self {
        let c = UClock::<T>::new();
        Self {
            v : [pair::<UTime,SysTime>::default();TRACE_SIZE],
            p : 0,
            f : false,
            e : (0f64,1e6),
            r : c.data,
            c : T::default()
        }
    }
}

impl<T:Clocker+Default> ClockTrace<T> {
    pub fn zero_e(&self,d:&UClockData) -> UClockData {
        if !self.f && self.p<TRACE_SIZE/64 {
            *d
        } else {
            let mut xd = *d;
            let pos = (self.p+TRACE_SIZE-1usize)%TRACE_SIZE;
            xd.cpu_start = self.v[pos].1;
            xd.self_start = d.from_sys(&xd.cpu_start);
            let (e0,e1) = if !self.f {
                error(&self.v[..pos],&xd)
            } else {
                error(&self.v,&xd)
            };
            println!("error {} {}, self.p {}",e0,e1,self.p);
            UClockData { 
                rate_nano:xd.rate_nano, 
                cpu_start:xd.cpu_start, 
                self_start:xd.self_start,//UTime(xd.self_start.0-(e0+0.5f64) as i64),
                quality:e1,
            }
        }        
    }
    pub fn to_next(&self) -> Option<pair<UTime,SysTime>> {
        if self.f && self.p==0 {
            Some(pair(self.r.self_start,self.r.cpu_start))
        } else {
            None
        }
    }
    pub fn reset(&mut self) {
        self.p = 0;
        self.f = false;
    }
}
#[derive(Default)]
pub struct TwoStage<T:Clocker+Default> {
    pub l0 : ClockTrace<T>,
    pub l1 : ClockTrace<T>,
}
impl<T:Clocker+Default> TwoStage<T> {
    pub fn one_ls(&mut self) -> Option<UClock<T>> {
        let mut tempc = T::default();
        let p = pair(tempc.now(),SysTime::now());
        if !self.l0.update(p) {
            println!("stage 0 not ready sleep {}ms", 100);
            thread::sleep(Duration::from_millis(100));
            None
        } else {
            if self.l0.e.1>1e8 {
                self.l1.reset();
            }
            let s = self.l0.e.1.sqrt().sqrt();
            let mut si = (1e4/s) as u64;
            if si < 10 { si = 10u64; }
            if si > 10000 { si = 10000u64; }
            println!("sleep {}ms", si);
            thread::sleep(Duration::from_millis(si));
            match self.l0.to_next() {
                None => {},
                Some(p) => {self.l1.update(p);},
            }
            let d = {
                if !self.l1.f && self.l1.p<3 {
                    self.l0.r
                } else {
                    merge(&self.l0.r,&self.l1.r,0.3f64)
                    //self.l0.r
                }
            };
            Some(UClock::<T>{ data: self.l0.zero_e(&d), clock:T::default()})    
        }

    }
    pub fn state(&self) -> (bool,bool) {(self.l0.f||self.l0.p>3,self.l1.f||self.l1.p>3)}
}


fn mi64_U(a:&UTime,b:&UTime,alf:f64) -> UTime {
    UTime(((a.0 as f64) * alf + (1.0f64-alf)*(b.0 as f64) + 0.5) as i64)
}
fn mi64_S(a:&SysTime,b:&SysTime,alf:f64) -> SysTime {
    SysTime(((a.0 as f64) * alf + (1.0f64-alf)*(b.0 as f64) + 0.5) as i64)
}
fn merge(a:&UClockData,b:&UClockData,alf:f64) -> UClockData {
    UClockData { 
        rate_nano:a.rate_nano*alf+(1.0f64-alf)*b.rate_nano, 
        self_start:a.self_start,
        cpu_start: a.cpu_start,
        quality: a.quality,
    }
}